import queue
import logging
import threading
import sys 
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")

from algorithm.CarControl import CarControl


class Mcu_com:
    def __init__(self):
        self.run = False
        self.__log_init()


    def __log_init(self):
        self.logger = logging.getLogger("Mcu_communication")


    def open(self, serialDevice):
        self.logger.trace('open.enter')
        self.mcu = CarControl(serialDevice)
        self.logger.trace('open.done')
        return self.mcu.serialObject.is_open


    def close(self):
        self.logger.trace('close.enter')
        self.mcu.serialObject.close()
        self.logger.trace('close.done')


    def thread(self):
        self.logger.trace('thread.enter')
        number = 0

        while self.run:
            try:
                speed, dspeed, number = self.inqueue_speed.get(timeout=0.1)
                self.mcu.setSpeed(speed, dspeed)
                self.logger.trace("speed set: %f, %f", speed, dspeed)
                # self.logger.spam("speed get: %d", number)
                
                # because the imu data will be required when car wants to adjust
                # so when speed==0 and dspeed==0,we set the imu data
                # if speed == 0 and dspeed == 0:
                #     imu_data = self.mcu.getMPU()    
                #     self.logger.trace("imu data: %s", imu_data)
                #     if self.outqueue1.full():
                #         self.logger.warning("imu queue full!")
                #         self.outqueue1.get(timeout=0.1)
                #     self.outqueue1.put(imu_data, timeout=0.1)    
            
            except queue.Empty as e:
                self.logger.spam('input queue empty!')
            except Exception as e:
                self.logger.error('input queue error!')

            if not self.inqueue_info.empty():
                try:
                    # print the information to the screen
                    line, information, number = self.inqueue_info.get(timeout=0.1)
                    if line < 0 and information == "if alive":
                        sw = self.mcu.getSwStatus()
                        try:
                            if self.outqueue1.full():
                                self.outqueue1.get_nowait()
                            self.outqueue1.put([sw], timeout=0.1)
                            # self.logger.spam(f"sw status: {sw}")
                        except Exception as e:
                            self.logger.error('output queue error: %s', e)
                        continue
                    elif line < 0:
                        self.mcu.sendBias(information)
                        continue
                    information = str(information)
                    self.mcu.setMessage(line, information)
                    self.logger.trace("information set: %s", information)
                    self.logger.info("information get: %d", number)
                except queue.Empty as e:
                    self.logger.trace('input queue empty!')
                except Exception as e:
                    self.logger.error('input queue error!')

                

        self.logger.trace('thread.done')


    def start(self):
        self.logger.trace('start.enter')

        if not self.mcu.serialObject.is_open:
            self.logger.error("mcu not connected, can not start!")
            return False

        self.run = True
        self.handle = threading.Thread(target=self.thread)
        self.handle.start()

        self.logger.trace('start.done')
        return True


    def stop(self):
        self.logger.trace('stop.enter')
        self.run = False
        self.mcu.setSpeed(0, 0)
        self.handle.join()
        self.logger.trace('stop.done')


    def input_queue_set(self, input_queue_list):
        self.logger.trace('intput_queue_set.enter')

        if self.run:
            self.logger.error("mcu is opened, please close the serial first!")
            return False

        if not isinstance(input_queue_list, list):
            input_queue_list = [input_queue_list]

        for input_queue in input_queue_list:
            if not isinstance(input_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False
            
        self.inqueue_speed = input_queue_list[0]
        self.inqueue_info = input_queue_list[1]

        self.logger.trace('intput_queue_set.done')
        return True


    def output_queue_set(self, output_queue_list):
        self.logger.trace('output_queue_set.enter')

        if not isinstance(output_queue_list, list):
            output_queue_list = [output_queue_list]

        for output_queue in output_queue_list:
            if not isinstance(output_queue, queue.Queue):
                self.logger.error("one queue is not a queue!")
                return False
            
        self.outqueue1 = output_queue_list[0]  # [if_alive]

        self.logger.trace('output_queue_set.done')
        return True

